%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Method:       Computes the quadratic-form coefficients for bond prices
%
% References:   Realdon, M (2006), 'Quadratic term structure models in
%               discrete time', Finance Research Letters 3, pp. 277-289
%
% Author:       Andrew Meldrum and Martin Andreasen
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [model,errorMes] = solveQTSM_Realdon(alpha,beta,psi,mu,phi,sigma,setup)

%% Allocating memory 
maxMat     = max(setup.matSelect);            % max maturity
ny         = size(setup.matSelect,2);         % number of bond yields
nxtil      = 2*setup.nm + setup.nn;           % number of pricing factors

% Law of motion for factors under Q 
h0Q = phi*mu;
hxQ = eye(nxtil)-phi;


% Check if co-variance matrix for innovations is positive semi-definite
sigma2 = sigma*sigma';
if any(eig(sigma2)<0)
    errorMes = 1;
    model = [];
    return
end
%% Compute bond pricing coefficients
% Initialise memory for bond price coefficients in state 1
A1 = zeros(1,maxMat);
B1 = zeros(nxtil,maxMat);
C1 = zeros(nxtil,nxtil,maxMat);

% Compute the inverse of sigma squared (sigma is diagonal)
% invsigma2 = diag(1./diag(sigma.^2));
% for non-diagonal case
invsigma2 = sigma2\eye(nxtil);
if any(isnan(reshape(invsigma2,nxtil^2,1))==1) || any(isinf(reshape(invsigma2,nxtil^2,1))==1)
    errorMes = 1;
    model = [];
    return
end
detSigma  = det(sigma);

% Compute recursive coefficients for bond prices
for n = 1:maxMat   
    % One-period bond prices use the initial conditions
    if n == 1
        % Compute the recursive bond pricing coefficients.  All terms
        % involving multiplication by A0, B0 or C0 are zero.
        A1(1) = - alpha;
        B1(:,1) = - beta;
        C1(:,:,1) = - psi;

    % Longer bond prices depend on the previous maturity
    else
        % Compute the gamma matrix in Realdon eq (19)
        gamma = (invsigma2 - 2.*C1(:,:,n-1))^(-.5);
        %gammasq = invsigma2 - 2.*C1(:,:,n-1);
        %[vgamma,d] = eig(gammasq);
        %gamma = vgamma*(d^(-0.5))*inv(vgamma);

        % Check for negative values of gamma
        if det(gamma) < 0 || sum(sum(abs(imag(gamma)))) ~= 0 || isnan(det(gamma))
            errorMes = 1;
            model = [];
            return  
        end

        % Initialise memory for the summation terms in each of the
        % difference equations (Realdon eqs (20)-(22))
        aSum = 0;
        bSum = 0;
        cSum = 0;
        %aSumMMA = 0;
        %bSumMMA = 0;
        %cSumMMA = 0;
        
        % Compute those summation terms
        for i = 1:nxtil
            gammaisq = gamma(:,i)*gamma(:,i)';
            aSum = aSum + 0.5.*B1(:,n-1)'*gammaisq*B1(:,n-1) +...
                2.*h0Q'*C1(:,:,n-1)*gammaisq*B1(:,n-1)       +...
                2.*h0Q'*C1(:,:,n-1)*gammaisq*C1(:,:,n-1)*h0Q;
            bSum = bSum + 2.*B1(:,n-1)'*gammaisq*C1(:,:,n-1)'*hxQ +...
                4.*h0Q'*C1(:,:,n-1)*gammaisq*C1(:,:,n-1)*hxQ;
            cSum = cSum + 2.*hxQ'*C1(:,:,n-1)*gammaisq*C1(:,:,n-1)*hxQ;
            % Test by Martin
            %aSumMMA = aSumMMA + 0.5*((B1(:,n-1)'*gamma(:,i))^2  ...
            %    + 2*B1(:,n-1)'*gamma(:,i)*h0Q'*C1(:,:,n-1)*gamma(:,i) ...
            %    + 2*h0Q'*C1(:,:,n-1)*gamma(:,i)*B1(:,n-1)'*gamma(:,i) ...
            %    + 4*(h0Q'*C1(:,:,n-1)*gamma(:,i))^2);
            %bSumMMA = bSumMMA + 2*B1(:,n-1)'*gamma(:,i)*gamma(:,i)'*C1(:,:,n-1)*hxQ ...
            %    + 4*h0Q'*C1(:,:,n-1)*gamma(:,i)*gamma(:,i)'*C1(:,:,n-1)*hxQ;
            %cSumMMA = cSumMMA + 2.*hxQ'*C1(:,:,n-1)*(gamma(:,i)*gamma(:,i)')*C1(:,:,n-1)*hxQ;  
        end
    
        % Compute the recursive bond pricing coefficients
        A1(n) = - alpha + A1(n-1) + B1(:,n-1)'*h0Q + h0Q'*C1(:,:,n-1)*h0Q...
            + log(det(gamma)./abs(detSigma)) + aSum;
        B1(:,n) = -beta + (B1(:,n-1)'*hxQ)' + 2.*(h0Q'*C1(:,:,n-1)*hxQ)' + bSum';
        C1(:,:,n) = - psi + hxQ'*C1(:,:,n-1)*hxQ + cSum;
    end
end


%% Reporting output in the desired format: annualized yields in pct.
% Spot: y_{n,t} = -(1/n)*p_{n,t}
nx         = nxtil;

% The short rate
r0   = 12*(-A1(1)/1);
rx   = 12*(-B1(:,1)'/1);
rxx  = 12*(-reshape(C1(:,:,1),1,nx^2)/1);
g0   = 12*(-A1(1,setup.matSelect)'./setup.matSelect');
gx   = 12*(-B1(:,setup.matSelect)'./repmat(setup.matSelect',1,nx));
gxx  = zeros(ny,nx^2);
for i=1:ny
    gxx(i,:) = 12*(-reshape(C1(:,:,setup.matSelect(1,i)),nx^2,1)./repmat(setup.matSelect(1,i),1,nx^2)');
end

% Storing output
model = struct('ny',length(g0),'nx',nx,'g0',g0,'gx',gx,'gxx',gxx,'h0Q',h0Q,'hxQ',hxQ,'matSelect',setup.matSelect,...
    'r0',r0,'rx',rx,'rxx',rxx,...
    'alpha',alpha,'beta',beta,'psi',psi,'phi',phi,'mu',mu,'sigmaxtil',sigma);

% Report no error if applicable
errorMes = 0;


end